#pragma once
#include <Math\Vector2.hpp>

//sub matrix support, works like matlab

namespace zzz{
template<typename T,class Major>
class zMatrixBaseR;

class EndlessArray
{
public:
  explicit EndlessArray(int begin):begin_(begin){}
  int begin_;
};
inline const EndlessArray Colon(int begin=0)
{
  return EndlessArray(begin);
}

//continuous column vector
template<typename T,class Major>
class zVConstantContinuousVector : public zMatrixBaseR<T,Major>
{
  using zMatrixBaseR<T,Major>::rows_;
  using zMatrixBaseR<T,Major>::cols_;
public:
  // constructor
  explicit zVConstantContinuousVector(T v0, T v1, T step=1)
  :zMatrixBaseR<T,Major>(zuint((v1-v0)/step+1),1),V0_(v0),V1_(v1),Step_(step){}
  // operator()
  using zMatrixBaseR<T,Major>::operator();
  const T operator()(zuint r, zuint c) const {
    CheckRange(r, c);
    return V0_+r*Step_;
  }
public:
  T V0_,V1_,Step_;
};

template<typename T,class Major>
inline zVConstantContinuousVector<T,Major> Colon(T begin, T end, T step=1)
{
  return zVConstantContinuousVector<T,Major>(begin,end,step);
}

inline zVConstantContinuousVector<double,zColMajor> Colond(double begin, double end, double step=1)
{
  return zVConstantContinuousVector<double,zColMajor>(begin,end,step);
}

inline zVConstantContinuousVector<float,zColMajor> Colonf(float begin, float end, float step=1)
{
  return zVConstantContinuousVector<float,zColMajor>(begin,end,step);
}

inline zVConstantContinuousVector<int,zColMajor> Colon(int begin, int end, int step=1)
{
  return zVConstantContinuousVector<int,zColMajor>(begin,end,step);
}

inline zVConstantContinuousVector<int,zColMajor> Colon(const Vector2i &range, int step=1)
{
  return zVConstantContinuousVector<int,zColMajor>(range[0],range[1],step);
}

template<typename T,class Major>
class zHConstantContinuousVector : public zMatrixBaseR<T,Major> //should not matter if rowmajor or colmajor
{
  using zMatrixBaseR<T,Major>::rows_;
  using zMatrixBaseR<T,Major>::cols_;
public:
  //constructor
  explicit zHConstantContinuousVector(T v0, T v1, T step=1)
  :zMatrixBaseR<T,Major>(1,zuint((v1-v0)/step+1)),V0_(v0),V1_(v1),Step_(step){}
  //operator()
  using zMatrixBaseR<T,Major>::operator();
  const T operator()(zuint r, zuint c) const
  {
    CheckRange(r, c);
    return V0_ + c * Step_;
  }
public:
  T V0_, V1_, Step_;
};

template<typename T,class Major>
inline zHConstantContinuousVector<T,Major> HColon(T begin, T end, T step=1)
{
  return zHConstantContinuousVector<T,Major>(begin,end,step);
}

template<typename T,class Major>
class zSubMatrixR : public zMatrixBaseR<T,Major>
{
  using zMatrixBaseR<T,Major>::rows_;
  using zMatrixBaseR<T,Major>::cols_;
public:
  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const int r, const int c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseR<T,Major>(1,1),ri_(r),ci_(c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const int r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseR<T,Major>(1,mat.cols_-c.begin_),ri_(r),ci_(c.begin_) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const int r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseR<T,Major>(1,c.rows_),ri_(r),c_(&c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const EndlessArray &r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseR<T,Major>(mat.rows_-r.begin_,mat.cols_-c.begin_),ri_(r.begin_),ci_(c.begin_) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const EndlessArray &r, const int c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseR<T,Major>(mat.rows_-r.begin_,1),ri_(r.begin_),ci_(c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const EndlessArray &r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseR<T,Major>(mat.rows_-r.begin_,c.rows_),ri_(r.begin_),c_(&c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseR<T,Major>(r.rows_,c.rows_),r_(&r),c_(&c) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const int c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseR<T,Major>(r.rows_,1),r_(&r),ci_(c) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixR(const zMatrixBaseR<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseR<T,Major>(r.rows_,mat.cols_-c.begin_),r_(&r),ci_(c.begin_) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }


  const T operator()(zuint r, zuint c) const
  {
    CheckRange(r, c);
    int r2,c2;
    switch(rtype_) {
    case ZMAT_IDX_ARRAY: r2=(*r_)(r,0); break;
    case ZMAT_IDX_CONST: r2=ri_; break;
    case ZMAT_IDX_ENDLESS: r2=ri_+r; break;
    }
    switch(ctype_) {
    case ZMAT_IDX_ARRAY: c2=(*c_)(c,0); break;
    case ZMAT_IDX_CONST: c2=ci_; break;
    case ZMAT_IDX_ENDLESS: c2=ci_+c; break;
    }
    ZCHECK_GE(r2, 0);
    ZCHECK_LT(r2, (int)Mat_.rows_);
    ZCHECK_GE(c2, 0);
    ZCHECK_LT(c2, (int)Mat_.cols_);
    return Mat_(zuint(r2),zuint(c2));
  }
private:
  const zMatrixBaseR<T,Major> &Mat_;
  enum {ZMAT_IDX_ARRAY,ZMAT_IDX_CONST,ZMAT_IDX_ENDLESS} rtype_,ctype_;
  int ri_, ci_;
  const zMatrixBaseR<int,Major> *r_,*c_;
};

template<typename T,class Major>
class zSubMatrixW : public zMatrixBaseW<T,Major>
{
  using zMatrixBaseR<T,Major>::rows_;
  using zMatrixBaseR<T,Major>::cols_;
public:
  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const int r, const int c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseW<T,Major>(1,1),ri_(r),ci_(c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const int r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseW<T,Major>(1,mat.cols_-c.begin_),ri_(r),ci_(c.begin_) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const int r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_CONST),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseW<T,Major>(1,c.rows_),ri_(r),c_(&c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const EndlessArray &r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseW<T,Major>(mat.rows_-r.begin_,mat.cols_-c.begin_),ri_(r.begin_),ci_(c.begin_) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const EndlessArray &r, const int c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseW<T,Major>(mat.rows_-r.begin_,1),ri_(r.begin_),ci_(c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const EndlessArray &r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_ENDLESS),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseW<T,Major>(mat.rows_ - r.begin_, c.rows_),ri_(r.begin_),c_(&c) {
      ZCHECK_GE(ri_, 0);
      ZCHECK_LT(ri_, (int)mat.rows_);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const zMatrixBaseR<int,Major> &c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_ARRAY),
    zMatrixBaseW<T,Major>(r.rows_,c.rows_),r_(&r),c_(&c) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_EQ(c.cols_, 1);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const int c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_CONST),
    zMatrixBaseW<T,Major>(r.rows_,1),r_(&r),ci_(c) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  zSubMatrixW(zMatrixBaseW<T,Major> &mat, const zMatrixBaseR<int,Major> &r, const EndlessArray &c):Mat_(mat),rtype_(ZMAT_IDX_ARRAY),ctype_(ZMAT_IDX_ENDLESS),
    zMatrixBaseW<T,Major>(r.rows_,mat.cols_-c.begin_),r_(&r),ci_(c.begin_) {
      ZCHECK_EQ(r.cols_, 1);
      ZCHECK_GE(ci_, 0);
      ZCHECK_LT(ci_, (int)mat.cols_);
  }

  const T operator()(zuint r, zuint c) const
  {
    CheckRange(r, c);
    int r2,c2;
    switch(rtype_)
    {
    case ZMAT_IDX_ARRAY: r2=(*r_)(r,0); break;
    case ZMAT_IDX_CONST: r2=ri_; break;
    case ZMAT_IDX_ENDLESS: r2=ri_+r; break;
    }
    switch(ctype_)
    {
    case ZMAT_IDX_ARRAY: c2=(*c_)(c,0); break;
    case ZMAT_IDX_CONST: c2=ci_; break;
    case ZMAT_IDX_ENDLESS: c2=ci_+c; break;
    }
    assert(r2>=0 && r2<(int)Mat_.rows_ && c2>=0 && c2<(int)Mat_.cols_);
    return Mat_(zuint(r2),zuint(c2));
  }
  T& operator()(zuint r, zuint c)
  {
    CheckRange(r, c);
    int r2,c2;
    switch(rtype_) {
    case ZMAT_IDX_ARRAY: r2=(*r_)(r,0); break;
    case ZMAT_IDX_CONST: r2=ri_; break;
    case ZMAT_IDX_ENDLESS: r2=ri_+r; break;
    }
    switch(ctype_) {
    case ZMAT_IDX_ARRAY: c2=(*c_)(c,0); break;
    case ZMAT_IDX_CONST: c2=ci_; break;
    case ZMAT_IDX_ENDLESS: c2=ci_+c; break;
    }
    assert(r2>=0 && r2<(int)Mat_.rows_ && c2>=0 && c2<(int)Mat_.cols_);
    return Mat_(zuint(r2),zuint(c2));
  }
//  using zMatrixBaseW<T,Major>::operator=;
  const zMatrixBaseW<T,Major>& operator=(const zSubMatrixW<T,Major>& other)
  {
    return zMatrixBaseW<T,Major>::operator=(static_cast<const zMatrixBaseR<T,Major>&>(other));
  }
  const zMatrixBaseW<T,Major>& operator=(const zMatrixBaseR<T,Major>& other)
  {
    return zMatrixBaseW<T,Major>::operator=(other);
  }
private:
  zMatrixBaseW<T,Major> &Mat_;
  enum {ZMAT_IDX_ARRAY,ZMAT_IDX_CONST,ZMAT_IDX_ENDLESS} rtype_,ctype_;
  int ri_, ci_;
  const zMatrixBaseR<int,Major> *r_,*c_;
};
}
